/*
 * Steer.c
 *
 *  Created on: Sep 10, 2023
 *      Author: LINEAR
 */

#include "zf_common_headfile.h"

// t = 0.5ms——————-舵机会转到 0 °
// t = 1.0ms——————-舵机会转到 45°
// t = 1.5ms——————-舵机会转到 90°
// t = 2.0ms——————-舵机会转到 135°
// t = 2.5ms——————-舵机会转到 180°

#define SERVO_PWM               TIM2_PWM_MAP1_CH1_A15                  // 定义主板上舵机对应引脚
#define SERVO_FREQ              (100)                                           // 定义主板上舵机频率  请务必注意范围 50-300
#define servo_mid  715 //中值715，左右变化70,右转数值变小
#define servo_mid_100 (1430)  //1430  变化140  100Hz对应10ms，中值1.5ms对应90度，1.5/10=15%,

inline void steer_init()
{
    pwm_init(SERVO_PWM, SERVO_FREQ, servo_mid_100 );

}

//废弃ad_least作为外部变量
inline void adc_steer(inductors * p_inductor,uint8_t ad_least[])
{
    p_inductor->sum_L=(int16)(ad_least[0]+ad_least[1]);//左电感值和
    p_inductor ->sum_R=(int16)(ad_least[5]+ad_least[6]);//右电感值和
    p_inductor ->sum_H=(int16)(ad_least[0]+ad_least[6]);//横电感值和
    p_inductor-> sum_slant=(int16)(ad_least[1]+ad_least[5]);//斜电感值和
    p_inductor ->sum_vertical=(int16)(ad_least[2]+ad_least[4]);//竖电感值和
    p_inductor  ->err_vertical=(int16)(abs(ad_least[2]-ad_least[4]));//竖电感差值
    p_inductor ->err_slant=(int16)(abs(ad_least[1]-ad_least[5]));//斜电感差值
    p_inductor ->err_H=(int16)(abs(ad_least[0]-ad_least[6]));//横电感差值

    p_inductor->ad_H = (sqrt(ad_least[0] * 100) - sqrt(ad_least[6] * 100)) * 27.0 / p_inductor  ->sum_H * 10;//计算 sum_H 距离差
    p_inductor->ad_slant = (sqrt(ad_least[1] * 100) - sqrt(ad_least[5] * 100)) * 18.0 / p_inductor  ->sum_slant * 10;//计算 sum_X 距离差

    p_inductor -> ad_result =  p_inductor->ad_H +  p_inductor->ad_slant; // 计算最终舵机控制信号值 ad_result

    if(Element_t.pdflag==0 && ((Element_t.hdflag>=0) && ((p_inductor->sum_L>200 && p_inductor->sum_R>200 && ad_least[3]>140) || (p_inductor->sum_L>200 && p_inductor->sum_R>200 && ad_least[3]>140) || Element_t.hdflag>0)))
    {
         Roundabout(&Element_t);//圆环判定函数，圆环写完后把if放在这里，不是圆环就不要进函数，要不然浪费资源
    }

    p_inductor->fuzzy_result = FUZZP_GET(p_inductor -> ad_result, (p_inductor->ad_result -  p_inductor->ad_last));//舵机控制信号进行模糊PID控制，调整舵机的转动角度
    ad_pwm =  (p_inductor ->ad_result)*ad_P +  (p_inductor ->ad_result * p_inductor ->fuzzy_result )/ 20.0 + ( p_inductor ->ad_result- p_inductor ->ad_last)*ad_D + (p_inductor ->ad_result -  p_inductor ->ad_last) *  p_inductor ->fuzzy_result / 20.0 ;

  p_inductor ->ad_last =  p_inductor ->ad_result;//last error赋值
 if(ad_pwm>steer_max)ad_pwm=steer_max; //左转幅度大于允许最大变化量
else if(ad_pwm<-steer_max)ad_pwm= -steer_max;
pwm_set_duty(SERVO_PWM, servo_mid_100+ad_pwm/2.43);//50Hz->4.85 100Hz->2.43

}


